#ifndef SHOOTERANGLE_H
#define SHOOTERANGLE_H

#include "Commands/PIDSubsystem.h"
#include "WPILib.h"

//#define PRACTICE_BOT
/**
 *
 *
 * @author team308
 */
class ShooterAngle: public PIDSubsystem {
private:
	// It's desirable that everything possible under private except
	// for methods that implement subsystem capabilities
	// set the P, I, and D constants here
	
	/************************************
	POT POSITIONS
	----------------------------------
	PRACTICE BOT::
	Lowest:		2.25V	3.5deg	(Measured)
	2nd Lowest: 2.14V	7.4deg	(Measured)
	Highest: 	1.125V	39deg	(Measured)
	.0317 V/deg
	*************************************
	COMP BOT:
	Lowest:		2.567V	3   deg	(Measured)
	2nd Lowest: 2.14V	7.4 deg	(Measured)
	Highest: 	1.4 V	39.7deg	(Measured)
		*/
	bool started;
//#ifdef PRACTICE_BOT
//	static const double Kp = 0.175;
//	static const double Ki = 0.0;
//	static const double Kd = 0.0;
//	static const double minVoltage = 2.14;
//	static const double maxVoltage = 1.125;
//	static const double minAngle = 7.4;
//	static const double maxAngle = 39;
//#else
	static const double Kp = 0.2;
	static const double Ki = 0.0;
	static const double Kd = 0.0;
	static const double minVoltage = 2.8;
	static const double maxVoltage = 0.630;
	static const double minAngle = 1.5;
	static const double maxAngle = 42.0;
//#endif
public:
	void SetRelative(float a);
	static const float ShooterHome = 10;
	void SetPID(float P, float I, float D);
	AnalogChannel shooterAnglePot;
	Victor shooterAngleVictor;
	ShooterAngle();
	double ReturnPIDInput();
	void UsePIDOutput(double output);
	float ConvertToAngle(float voltage);
	float GetCurrentShooterAngle();
	void InitDefaultCommand();
};

#endif
